﻿package controller.model;

import java.rmi.RemoteException;
import java.rmi.server.UnicastRemoteObject;
import java.util.concurrent.CopyOnWriteArrayList;

import controller.acquaintance.*;
import controller.subsystemcom.MesCommunicator;
import client.sharedRessources.Order;

@SuppressWarnings("serial")
public class WorkCellControllerImpl extends UnicastRemoteObject implements I_WorkCellController_PackML, I_WorkCellController_Vision, I_WorkCellController_PackmlHandler, I_WorkCellController_OEE
{
	private I_Mes_WorkCellController mesCommunicator;
//	protected I_RobotController_WorkCellController robotController;
			
	private CopyOnWriteArrayList<Brick> bricks;

	private Order currentOrder;
	private int remainingYellow, remainingRed, remainingBlue;
	private long timeLastBrick = 0;
	private boolean currentOrderCompleted = false;
	
	private volatile boolean robotCanMove;
	
	private volatile int bricksUsed = 0, bricksEnteredConveyor = 0;
	
	protected WorkCellControllerImpl() throws RemoteException
	{
		super();
		bricks = new CopyOnWriteArrayList<Brick>();
		this.timeLastBrick = System.currentTimeMillis();
	}
	
	public void instantiateSubSystems()
	{
		mesCommunicator = MesCommunicator.getInstance();
//		robotController = RobotController.getInstance();
	}
	
	public int yellows(boolean change)
	{
		if (change)
			remainingYellow--;
		return remainingYellow;
	}

	public int reds(boolean change)
	{
		if (change)
			remainingRed--;
		return remainingRed;
	}

	public int blues(boolean change)
	{
		if (change)
			remainingBlue--;
		return remainingBlue;
	}	
	
	public boolean robotCanMove()
	{ return this.robotCanMove; }

	public void setCurrentOrderCompleted(boolean completed)
	{
		currentOrderCompletedAccess(false, completed);
	}

	public boolean isCurrentOrderCompleted()
	{
		return currentOrderCompletedAccess(true, false);
	}
	
	private synchronized boolean currentOrderCompletedAccess(boolean check, boolean completed)
	{
		if (!check)
		{
			this.currentOrderCompleted = completed;
		}
		return this.currentOrderCompleted;
	}

	public long getTimeSinceLastBrick()
	{
		return timeSinceLastBrickAccess(true, 0);
	}
	
	private synchronized long timeSinceLastBrickAccess(boolean check, long time)
	{
		if (!check)
			this.timeLastBrick = time;
		return this.timeLastBrick;
	}
	
	/*
	 * All methods below this line are methods implemented from the various interfaces
	 */	
	public void newYellowBrickSpotted(double x, double y, double angle, long time) throws RemoteException
	{
		Brick yellow = new Brick(BrickColor.YELLOW, x, y, angle, time);
		bricksAccess(true, yellow, false, null);
		accessBricksEnteredConveyor(false);
		timeSinceLastBrickAccess(false, System.currentTimeMillis());
	}

	public void newRedBrickSpotted(double x, double y, double angle, long time) throws RemoteException
	{
		Brick red = new Brick(BrickColor.RED, x, y, angle, time);
		bricksAccess(true, red, false, null);
		accessBricksEnteredConveyor(false);
		timeSinceLastBrickAccess(false, System.currentTimeMillis());
	}
	
	public void newBlueBrickSpotted(double x, double y, double angle, long time) throws RemoteException
	{
		Brick blue = new Brick(BrickColor.BLUE, x, y, angle, time);
		bricksAccess(true, blue, false, null);
		accessBricksEnteredConveyor(false);
		timeSinceLastBrickAccess(false, System.currentTimeMillis());
	}
	
	public synchronized CopyOnWriteArrayList<Brick> getBricks()
	{
		return bricksAccess(false, null, false, null);
	}
	
	public synchronized void removeBrick(Brick theBrick)
	{
		bricksAccess(false, null, true, theBrick);
	}
		
	private synchronized CopyOnWriteArrayList<Brick> bricksAccess(boolean addBrick, Brick toAdd, boolean removeBrick, Brick toRemove)
	{
		int brickToOld = 5000; //TODO
		long now = System.currentTimeMillis();
		
		if (addBrick && (now - toAdd.getTime()) < brickToOld)
			this.bricks.add(toAdd);
		
		if (removeBrick)
			this.bricks.remove(toRemove);
		
		for (Brick brick : bricks)
		{
	        if ((now - brick.getTime()) >  brickToOld)
	        	bricks.remove(brick);
	        else
	        	break;
		}
		
		return this.bricks;
	}
	
	public synchronized int accessBricksUsed(boolean checkAndReset) throws RemoteException
	{
		if (checkAndReset)
		{
			int temp = bricksUsed;
			bricksUsed = 0;
			return temp;
		}
		else
			bricksUsed++;
		System.out.println("Bricks used: " + bricksUsed);
		return 0;
	}
	
	public synchronized int accessBricksEnteredConveyor(boolean checkAndReset) throws RemoteException
	{
		if (checkAndReset)
		{
			int temp = bricksEnteredConveyor;
			bricksEnteredConveyor = 0;
			return temp;
		}
		else
			bricksEnteredConveyor++;
		System.out.println(" bricks entered conveyor used: " + bricksEnteredConveyor);
		return 0;
	}
	
	public void fetchNewOrder() throws RemoteException
	{
		currentOrder = mesCommunicator.getNewOrder();
		
		this.remainingBlue = currentOrder.getBlues();
		this.remainingRed = currentOrder.getReds();
		this.remainingYellow = currentOrder.getYellows();

		System.out.println("new order");

		setCurrentOrderCompleted(false);
		
		OrderHandler handler = new OrderHandler(this);
		handler.start();
	}

	public void orderCompleted(long time) throws RemoteException
	{
		mesCommunicator.orderCompleted(time, currentOrder.getOrderID(), currentOrder.getTicket());
	}

	public void sendLogPML_Idle(long time, String comment) throws RemoteException
	{
		mesCommunicator.sendLogPML_Idle(time, comment);
	}

	public void sendLogPML_Execute(long time, String comment) throws RemoteException
	{
		mesCommunicator.sendLogPML_Execute(time, comment);
		robotCanMove = true;
	}
	
	public void sendLogPML_Complete(long time, String comment) throws RemoteException
	{
//		orderCompleted(time);
		mesCommunicator.sendLogPML_Complete(time, comment);
	} 

	public void sendLogPML_Held(long time, String comment) throws RemoteException
	{
		mesCommunicator.sendLogPML_Held(time, comment);
	}

	public void sendLogPML_Suspended(long time, String comment) throws RemoteException
	{
		mesCommunicator.sendLogPML_Suspended(time, comment);
	}

	public void sendLogPML_Aborted(long time, String comment) throws RemoteException
	{
		mesCommunicator.sendLogPML_Aborted(time, comment);
	}

	public void sendLogPML_Stopped(long time, String comment) throws RemoteException
	{
		mesCommunicator.sendLogPML_Stopped(time, comment);
	}

	public void sendLogOrder_Start(long time, String comment) throws RemoteException
	{
		if (currentOrder != null)
			mesCommunicator.sendLogOrder_Start(time, currentOrder.getOrderID());
	}

	public void PML_state_Stopping(long time, String comment) throws RemoteException
	{
//		robotController.PML_state_Stopping(time, comment);
	}

	public void PML_state_Resetting(long time, String comment) throws RemoteException
	{
//		robotController.PML_state_Resetting(time, comment);
	}

	public void PML_state_Starting(long time, String comment) throws RemoteException
	{
		// Nothing happens. Start button on PackML interface is pressed once.
		// When pressed the second time sendLogPML_Execute is called.	
	}

	public void PML_state_Holding(long time, String comment) throws RemoteException
	{
//		robotController.PML_state_Holding(time, comment);
		robotCanMove = false;
	}

	public void PML_state_UnHolding(long time, String comment) throws RemoteException
	{
//		robotController.PML_state_UnHolding(time, comment);
	}

	public void PML_state_Completing(long time, String comment) throws RemoteException
	{
		robotCanMove = false;
//		robotController.PML_state_Completing(time, comment);
	}

	public void PML_state_Suspending(long time, String comment) throws RemoteException
	{
//		robotController.PML_state_Suspending(time, comment);
		robotCanMove = false;
	}

	public void PML_state_UnSuspending(long time, String comment) throws RemoteException
	{
//		robotController.PML_state_UnSuspending(time, comment);
	}
}

class OrderHandler extends Thread
{
	private WorkCellControllerImpl workCellController;
	private boolean performsPickUp;
	
	public OrderHandler(WorkCellControllerImpl workCellControllerImpl)
	{
		workCellController = workCellControllerImpl;
		try
		{
			workCellController.sendLogOrder_Start(System.currentTimeMillis(), "");
		} catch (RemoteException e)
		{
			e.printStackTrace();
		}
	}

	public void run()
	{
		while (true)
		{
			if (!performsPickUp)
			{
//				System.out.println("yellows: " + workCellController.yellows(false));
//				System.out.println("red: " + workCellController.reds(false));
//				System.out.println("blues: " + workCellController.blues(false));
				
				if (workCellController.yellows(false) < 1 &&
					workCellController.reds(false) < 1 &&
					workCellController.blues(false) < 1)
				{
					System.out.println("ORDER IS COMPLETED");
					workCellController.setCurrentOrderCompleted(true);
					return;
				}				
//				System.out.println("Brick on the conveyor " + workCellController.getBricks().size());
				boolean pickUpRequested = false;
				for (Brick brick : workCellController.getBricks())
				{
//					System.out.print(brick.getColor() + "\t");
					switch(brick.getColor())
					{
					case YELLOW:
						if (workCellController.yellows(false) > 0 &&
							workCellController.robotCanMove())
						{
							pickUpBrick(brick);
							pickUpRequested = true;
						}
						break;
					case RED:
						if (workCellController.reds(false) > 0 &&
							workCellController.robotCanMove())
						{
							pickUpBrick(brick);
							pickUpRequested = true;
						}
						break;
					case BLUE:
						if (workCellController.blues(false) > 0 &&
							workCellController.robotCanMove())
						{
							pickUpBrick(brick);
							pickUpRequested = true;
						}
						break;
					default:
						break;
					}
					if (pickUpRequested)
						break;
				}
				try {
					sleep(200);
				} catch (InterruptedException e)
				{
					e.printStackTrace();
				}
			}
			else
			{
				try
				{ Thread.sleep(500); }
				catch (InterruptedException e)
				{ e.printStackTrace(); }
			}
		}
	}
	
	private void pickUpBrick(Brick brick)
	{
		performsPickUp = true;

		RobotCoordinates coordinatesToUse = createPickup(brick.getX(), brick.getY());
		
//		workCellController.robotController.goToPickUpPosition(coordinatesToUse.getNextPrePickup());
		
		System.out.println("Robot go to: " + brick.getX() + " " + brick.getY() + " " + brick.getAngle());
		try
		{
			double dest = 1200 + (300 / 985 * brick.getX()); //in mm //TODO change 1200
			double speed = 0.15; //in ms
			long timeAlreadyWaited = System.currentTimeMillis() - brick.getTime();
			long waitPickUpTime = Math.round(dest / speed) - timeAlreadyWaited;
			
			Thread.sleep(waitPickUpTime);
		}
		catch (InterruptedException e)
		{ e.printStackTrace(); }
		
//		boolean success = workCellController.robotController.performPickUp(coordinatesToUse.getNextPickup());
		
		boolean success = true;
		
		if (success)
		{
			switch(brick.getColor())
			{
			case YELLOW:
				workCellController.yellows(true);
				break;
			case RED:
				workCellController.reds(true);
				break;
			case BLUE:
				workCellController.blues(true);
				break;
			default:
				break;
			}

			workCellController.removeBrick(brick);
			try
			{
				workCellController.accessBricksUsed(false);
			}
			catch (RemoteException e)
			{ e.printStackTrace(); }
		}

		performsPickUp = false;
	}
		
	private RobotCoordinates createPickup(double x, double y)
	{
		//receive x and y, generate hovering pose and pick up position
		int scalex=723;//maximum alue to receive for X from vision
		int scaley=100;//maximum alue to receive for Y from vision
		double spanx=0.1498;//full lenght to reach over X for the robot				
		double spany=0.704;//full lenght to reach over Y for the robot				
		double offsetx=0.329;//minimum value to reach when X=0
		double offsety=0.524;//minimum value to reach when Y=0
		double slopey=0.183;//slope correction for the belt over Y
		double angle=0.1885;//skewed angle of the belt respect to the robot ¡¡RADIANS!! (~10.8°)		
		double zhover=0.040;//height to hover
		double xmap, ymap, zpick, zhov;
		xmap=x;
		ymap=y;	
		ymap=-ymap*spany/(scaley)+offsety;
		xmap=xmap*(spanx)/(scalex)+((y*slopey)/scaley)+offsetx;
		double ymp=ymap+(xmap)*Math.sin(angle);
		double xmp=xmap;
		//Calculation for Z heights... basically magic D:!!
		double xz=x*34/scalex+5;
		zpick=0.043751-(-0.0122*Math.pow(xz, 2)  +  0.5130*xz   -0.8679)*0.027064/4;
		zhov=zpick+zhover;
		//angle to be calculated when testing to retrieve information
		/**/
		double[] nextPrePickup={xmp, ymp, zhov, -2.80252, 1.35461, 0.00663};
		double[] nextPickup={xmp, ymp, zpick, -2.80252, 1.35461, 0.00663};
		return new RobotCoordinates(nextPickup, nextPrePickup);
		/*python version:
		xmap=x
		ymap=y	
		ymap=-ymap*0.704/(100)+0.524
		xmap=xmap*(0.344)/(100)+((y*0.1*0.183)/10)+0.125
		ymp=ymap+(xmap)*sin(0.1885)
		xmp=xmap
		xz=x*34/100+5
		zpick=0.043751-(-0.0122*pow(xz, 2)  +  0.5130*xz   -0.8679)*0.027064/4
		zhov=zpick+0.040
		*/
	}
}